#include "protocol.h"
#include "protocol_remote.h"
#include "commData/commData.h"
#include "protocol_camera.h"

#define   	local_group		(2)
const uint8_t msg_head_local[5] = {0x62,0x65,0x67,0x69,0x6E};
const uint8_t msg_tail_local[4] = {0x65,0x6E,0x64};
list_prot 	local_queue_tx;
mesg_prot	local_info[1];
uint8_t u8_screen_acknum = 0;

void queue_insert_local(pmesg_prot pmesg)
{
	uint8_t pos, status, len, i;
	plist_prot pqueue;
	pqueue = &local_queue_tx;
	pos = pqueue->head;
	if(mesg_max==pos)pos = 0;
	status = pmesg->status;
	if(0==status){
		len = pmesg->len;
		pqueue->mesg_queue[pos].len = len;
		pqueue->mesg_queue[pos].status = status;
		pqueue->mesg_queue[pos].ID = pmesg->ID;
		for(i=0; i<len; i++){
			pqueue->mesg_queue[pos].buffer[i] = pmesg->buffer[i];
		}
		pqueue->head++;
		if(mesg_max==pqueue->head)pqueue->head=0;
		if(pqueue->size<10)pqueue->size++;		
	}
}

static uint8_t local_process_NST(uint8_t len, uint8_t data[], pmesg_prot mesg)
{
	uint8_t i;
	mesg_prot Tmesg;
	Tmesg.status = 0;
	Tmesg.ID = mesg->ID+0x80;
	for(i=0; i<len; i++){
		Tmesg.buffer[i] = data[i];
	}
	queue_insert_local(&Tmesg);
	return 0;
}

static void local_runstate_control(pmesg_prot mesg)
{
	uint8_t count;
	uint8_t data_Tx[8];
	//len = mesg->len;
	count = 0;
	data_Tx[count++] = 0x21;
	data_Tx[count++] = mesg->buffer[0];
	data_Tx[count++] = mesg->buffer[1];
	switch(mesg->buffer[2]){
		case(1):
			u8_sys_status = 1;
			data_Tx[count++] = mesg->buffer[1];
			data_Tx[count++] = mesg->buffer[2];
			data_Tx[count++] = 0;
		break;
		case(2):
			u8_sys_status = 0;
			data_Tx[count++] = mesg->buffer[1];
			data_Tx[count++] = mesg->buffer[2];
			data_Tx[count++] = 0;
		break;
		default:			
			data_Tx[count++] = 0xFF;			
		break;
	}
	local_process_NST(count, data_Tx, mesg);
}

static void local_directions_control(pmesg_prot mesg)
{
	uint8_t count;
	uint8_t data_Tx[8];
	//len = mesg->len;
	count = 0;
	data_Tx[count++] = 0x21;
	data_Tx[count++] = mesg->buffer[0];
	data_Tx[count++] = mesg->buffer[1];
	switch(mesg->buffer[2]){
		case(1):
			u8_sys_direction = mesg->buffer[2];			
			data_Tx[count++] = mesg->buffer[2];
			data_Tx[count++] = 0;
		break;
		case(2):
			u8_sys_direction = mesg->buffer[2];
			data_Tx[count++] = mesg->buffer[2];
			data_Tx[count++] = 0;
		break;
		case(3):
			u8_sys_direction = mesg->buffer[2];
			data_Tx[count++] = mesg->buffer[2];
			data_Tx[count++] = 0;
		break;
		case(4):
			u8_sys_direction = mesg->buffer[2];
			data_Tx[count++] = mesg->buffer[2];
			data_Tx[count++] = 0;
		break;
		default:			
			data_Tx[count++] = 0xFF;			
		break;
	}
	local_process_NST(count, data_Tx, mesg);
}

static void local_speed_control(pmesg_prot mesg)
{
	uint8_t count;
	uint8_t data_Tx[8];
	//len = mesg->len;
	count = 0;
	data_Tx[count++] = 0x21;
	data_Tx[count++] = mesg->buffer[0];
	data_Tx[count++] = mesg->buffer[1];
	
	u16_sys_speed = mesg->buffer[2];
	u16_sys_speed = (u16_sys_speed<<8)+mesg->buffer[3];
	data_Tx[count++] = mesg->buffer[2];
	data_Tx[count++] = mesg->buffer[3];
	data_Tx[count++] = 0;
	local_process_NST(count, data_Tx, mesg);
}

static void local_grap_control(pmesg_prot mesg)
{
	uint8_t count;
	uint8_t data_Tx[8];
	//len = mesg->len;
	count = 0;
	data_Tx[count++] = 0x21;
	data_Tx[count++] = mesg->buffer[0];
	data_Tx[count++] = mesg->buffer[1];
	switch(mesg->buffer[2]){
		case(1):
			u8_sys_grap = mesg->buffer[2];			
			data_Tx[count++] = mesg->buffer[2];
			data_Tx[count++] = 0;
		break;
		case(2):
			u8_sys_grap = mesg->buffer[2];
			data_Tx[count++] = mesg->buffer[2];
			data_Tx[count++] = 0;
		break;
		case(3):
			u8_sys_grap = mesg->buffer[2];
			data_Tx[count++] = mesg->buffer[2];
			data_Tx[count++] = 0;
		break;
		case(4):
			u8_sys_grap = mesg->buffer[2];
			data_Tx[count++] = mesg->buffer[2];
			data_Tx[count++] = 0;
		break;
		default:			
			data_Tx[count++] = 0xFF;			
		break;
	}
	local_process_NST(count, data_Tx, mesg);
}

static void local_debug_control(pmesg_prot mesg)
{
	uint8_t count;
	uint8_t data_Tx[8];
	//len = mesg->len;
	count = 0;
	data_Tx[count++] = 0x21;
	data_Tx[count++] = mesg->buffer[0];
	data_Tx[count++] = mesg->buffer[1];
	switch(mesg->buffer[2]){
		case(0):
			u8_sys_debug = mesg->buffer[2];			
			data_Tx[count++] = mesg->buffer[2];
			data_Tx[count++] = 0;
		break;
		case(1):
			u8_sys_debug = mesg->buffer[2];
			data_Tx[count++] = mesg->buffer[2];
			data_Tx[count++] = 0;
		break;
		default:			
			data_Tx[count++] = 0xFF;			
		break;
	}
	local_process_NST(count, data_Tx, mesg);
}
static void local_process_control(pmesg_prot mesg)
{
	uint8_t len, count;
	uint8_t data_Tx[8];
	len = mesg->len;
	count = 0;
	switch(mesg->buffer[1]){
		case(0x00)://runstate control
			//local_runstate_control(mesg);			
		//break;
		case(0x01)://direction control
			//local_directions_control(mesg);
		//break;
		case(0x02)://speed control
			//local_speed_control(mesg);
		//break;
		case(0x03)://grap control
			//local_grap_control(mesg);
			data_Tx[count++] = 0x21;
			data_Tx[count++] = mesg->buffer[0];
			data_Tx[count++] = 0xFF;
			local_process_NST(count, data_Tx, mesg);
		break;
		case(0x04)://debug control
			local_debug_control(mesg);
		break;
		default:
			//main sid not support			
			data_Tx[count++] = 0x21;
			data_Tx[count++] = mesg->buffer[0];
			data_Tx[count++] = 0xFF;
			local_process_NST(count, data_Tx, mesg);
		break;
	}
}
static void local_process_status(pmesg_prot mesg)
{
	uint8_t count;
	uint8_t data_Tx[16];
	//len = mesg->len;
	count = 0;
	data_Tx[count++] = 0x21;
	data_Tx[count++] = mesg->buffer[0];
	data_Tx[count++] = mesg->buffer[1];
	switch(mesg->buffer[2]){
		case(0x01)://system status
			data_Tx[count++] = mesg->buffer[2];
			data_Tx[count++] = 0;
			data_Tx[count++] = u8_sys_status;
		break;
		case(0x02)://motor status
			data_Tx[count++] = mesg->buffer[2];
			data_Tx[count++] = 0;
			data_Tx[count++] = u8_sportmotor_status;
			data_Tx[count++] = u8_grapmotor_status;
			data_Tx[count++] = u8_upmotor_status;
		break;
		case(0x03)://screen status
			data_Tx[count++] = mesg->buffer[2];
			data_Tx[count++] = 0;
			data_Tx[count++] = u8_screen_status;
		break;
		case(0x04)://camera status
			data_Tx[count++] = mesg->buffer[2];
			data_Tx[count++] = 0;
			data_Tx[count++] = u8_camera_status;
		break;
		case(0xFF)://ALL Status
			data_Tx[count++] = mesg->buffer[2];
			data_Tx[count++] = 0;
			data_Tx[count++] = u8_sys_status;
			data_Tx[count++] = u8_sportmotor_status;
			data_Tx[count++] = u8_grapmotor_status;
			data_Tx[count++] = u8_upmotor_status;
			data_Tx[count++] = u8_screen_status;
			data_Tx[count++] = u8_camera_status;
		break;
		default:
			//main sid not support
			data_Tx[count++] = 0xFF;			
		break;
	}
	local_process_NST(count, data_Tx, mesg);
}
static void local_process_config(pmesg_prot mesg)
{
	uint8_t count;
	uint8_t data_Tx[8];
	count = 0;
	data_Tx[count++] = 0x21;
	data_Tx[count++] = mesg->buffer[0];
	data_Tx[count++] = 0xFF;
	local_process_NST(count, data_Tx, mesg);
}
static void local_process_resp(pmesg_prot mesg)
{
	uint8_t count;
	uint8_t data_Tx[8];
	count = 0;
	data_Tx[count++] = 0x21;
	data_Tx[count++] = 0xFF;
	local_process_NST(count, data_Tx,mesg);
}
static void local_process_heartRx(pmesg_prot mesg)
{
	u8_screen_acknum = 0;
}
static void local_process_ErrorRx(pmesg_prot mesg)
{
	u8_screen_status = 0;
}

uint8_t local_process(pmesg_prot mesg)
{
	uint8_t count;
	uint8_t data_Tx[8];
	//len = mesg->len;
	switch(mesg->buffer[0]){
		case(0x10)://control
			local_process_control(mesg);
		break;
		case(0x20)://status check
			local_process_status(mesg);
		break;
		case(0x30)://
			local_process_config(mesg);
		break;
		case(0x21):
			local_process_resp(mesg);
		break;
		case(0x22):
			local_process_heartRx(mesg);
		break;
		case(0x25):
			local_process_ErrorRx(mesg);
		break;
		default:
			//main sid not support
			count = 0;
			data_Tx[count++] = 0x21;
			data_Tx[count++] = 0xFF;
			local_process_NST(count, data_Tx, mesg);
		break;
	}
	return 0;
}

uint8_t local_rxFunc(void)
{
	return 0;	
}

static uint8_t local_frametx(pmesg_prot mesg)
{
	uint8_t i, index, checksum;
	uint8_t buffer[256];
	uint32_t id;
	//checksum get
	checksum = 0;
	for(i=0; i<mesg->len; i++){
		checksum += mesg->buffer[i];
	}
	mesg->buffer[mesg->len] = checksum;
	//mesg->len++;//first byte
	mesg->len++;//checksum
	index = 0;
	//mesg_head insert
	for(i=0;i<5; i++){
		buffer[index++] = msg_head_local[i];
	}
	//ID insert
	for(i=0; i<4; i++){
		buffer[index++] = (uint8_t)((mesg->ID)>>(8*i));
	}
	//len insert
	buffer[index++] = mesg->len;
	//buffer insert
	for(i=0; i<mesg->len; i++){
		buffer[index++] = mesg->buffer[i];
	}
	//mesg_tail insert
	for(i=0;i<3; i++){
		buffer[index++] = msg_tail_local[i];
	}
	//tx frame
	//...
	return 0;
}
uint8_t local_txFunc(void)
{
	uint8_t result;
	uint8_t size, pos, status, len;
	plist_prot pqueue;
	//second, rx data pro
	pqueue = &local_queue_tx;
	size = pqueue->size;
	while(pqueue->size>0){
		pos = pqueue->tail;
		if(pos>=mesg_max)pos=0;
		//result pro
		result = local_frametx(&(pqueue->mesg_queue[pos]));
		pqueue->tail++;
		if(pqueue->tail>=mesg_max)pqueue->tail=0;		
		pqueue->size--;
	}
	return result;
}

void local_rxFunc_callback(uint8_t data_rx)
{
	uint16_t index ;
	static uint32_t id_last;
	static uint8_t startflag;
	static uint8_t count = 0;
	static uint8_t len = 0;
	static uint8_t step = 0;
	switch(step){
		case(0):
			//first, start flag check
			if(0==local_info[0].status){
				if(msg_head_local[count]==data_rx){
					count++;
				}else{
					count = 0;
				}
				if(5==count){
					step = 1;
					local_info[0].status=1;
					local_info[0].ID = 0;
					local_info[0].len = 0;
					count = 0;
				}
			}
		break;
		case(1):
			//second, id check
			if(count<4){
				local_info[0].ID = (local_info[0].ID<<8)+data_rx;
			}else{
				//third, len check
				step = 2;
				len = local_info[0].len = data_rx;
				count = 0;
			}
			count++;
		break;
		case(2):
			//four, data buffer
			if(count<(len)){
				local_info[0].buffer[count++] = data_rx;
			}else{
				//five, checksum
				local_info[0].buffer[count++] = data_rx;
				step = 3;
				count = 0;
			}
		break;
		case(3):
			//six, end check
			if(1==local_info[0].status){
				if(msg_tail_local[count]==data_rx){
					count++;
				}else{
					count = 0;
					step = 0;
					local_info[0].status=0;
				}
				if(3==count){
					step = 0;
					local_info[0].status=0;
					count = 0;
					//insert data
					list_insert(&local_info[index],local_group);
				}
			}else{
				step = 0;
				local_info[0].status=0;
				count = 0;
			}
		break;
		default:
			step = 0;
			local_info[0].status=0;
			count = 0;
		break;
	}
}

/*
Cmds
*/
uint8_t local_heart_Tx(void)
{
	uint8_t count, i;
	mesg_prot Tmesg;
	Tmesg.status = 0;
	Tmesg.ID = 0x100;
	count = 0;
	Tmesg.buffer[count++] = 0x22;
	Tmesg.buffer[count++] = u8_sys_status;
	Tmesg.buffer[count++] = u8_sys_workstatus;
	Tmesg.buffer[count++] = u8_bms_status;
	Tmesg.buffer[count++] = u8_screen_status;
	Tmesg.buffer[count++] = u8_sportmotor_status;
	Tmesg.buffer[count++] = u8_grapmotor_status;
	Tmesg.buffer[count++] = u8_upmotor_status;
	for(i=0; i<camera_slaveNodes; i++){
		Tmesg.buffer[count++] = camera_result[i].workstatus;
		Tmesg.buffer[count++] = camera_result[i].runstatus;
	}
	queue_insert_local(&Tmesg);
	return 0;
}

